package edu.tfh.s2.ehkbot.unitTest;

import static org.junit.Assert.*;

import org.junit.BeforeClass;
import org.junit.Test;

import edu.tfh.s2.ehkbot.EHKBotMain;
import edu.tfh.s2.ehkbot.EHKBotMain.SimulationNichtMoeglich;
import edu.tfh.s2.ehkbot.gerete.Fahrgestell;
import edu.tfh.s2.ehkbot.gerete.sensoren.IndoorGPS;
import edu.tfh.s2.ehkbot.steuerung.Position;
import edu.tfh.s2.ehkbot.steuerung.Richtung;

public class GPSTest {
	/**
	 * Wird vor den eigendlichen Tests aufgerufen
	 * 
	 * @throws SimulationNichtMoeglich
	 */
	@BeforeClass
	public static void init() throws SimulationNichtMoeglich {
		EHKBotMain.enableSimulation();
	}

	/**
	 * Führt mehrere Test der Funktion bewegtSich(..) durch
	 */
	@Test
	public void bewegtSich() {
		int[] offsets = { 1, 2, 3, 4, 5, 6, 7, 8, 10, 20, 90, 120, 179, 180, 181, 270 };
		for (int i = 0; i < offsets.length; ++i) {
			bewegtSich(offsets[i]);
		}

	}

	/**
	 * Testet ob sich der EHKBot bewegen kann
	 * 
	 * @param offset
	 *            Die Position an die sich der Bot bewegen soll.
	 */
	public void bewegtSich(int offset) {
		Fahrgestell fahrgestell = EHKBotMain.getInstance().getEHKBot().getFahrgestell();
		fahrgestell.dreheBot(new Richtung(0));
		Position p = IndoorGPS.getInstance().getCurrentPosition();
		fahrgestell.fahreBot(offset);
		if (p.equals(IndoorGPS.getInstance().getCurrentPosition()))
			fail("Bot hat sich nicht bewegt");
	}

	/**
	 * Führt mehrere Tests der Funktion drehtSich(..) durch
	 */
	@Test
	public void drehtSich() {
		int[] offsets = { 1, 2, 3, 4, 5, 6, 7, 8, 10, 20, 90, 120, 179, 180, 181, 270, 359 };
		for (int i = 0; i < offsets.length; ++i) {
			drehtSich(offsets[i]);
		}
	}

	/**
	 * Überprüft ob sich der EHKBot drehen kann
	 * 
	 * @param offset
	 *            Die "Richtung" (in Grad) in die gedreht werden soll
	 */
	public void drehtSich(int offset) {
		Fahrgestell fahrgestell = EHKBotMain.getInstance().getEHKBot().getFahrgestell();
		fahrgestell.dreheBot(new Richtung(0));
		double grad = IndoorGPS.getInstance().getRichtung().getAlsRad();
		fahrgestell.dreheBot(new Richtung(offset));
		String msg = String.format(
			"richtung hat sich nicht geändert (vorher%f ; nachher %f)",
			grad,
			IndoorGPS.getInstance().getRichtung().getAlsRad());
		assertFalse(msg, grad == IndoorGPS.getInstance().getRichtung().getAlsRad());

	}

	/**
	 * Führt mehrere Tests der Funktion drehtSichRichtig(..) aus
	 */
	@Test
	public void drehtSichRichtig() {
		double[] offsets = { 0.0, Math.PI * 0.1, Math.PI * 0.5, Math.PI * 1, Math.PI * 0.999,
						Math.PI * 1.001, Math.PI * 2 };
		for (int i = 0; i < offsets.length; ++i) {
			drehtSichRichtig(offsets[i]);
		}

	}

	/**
	 * Überprüft wie der EHKBot auf Minimaländerungen reagiert
	 * 
	 * @param offset
	 *            "Minimaländerung" des Drehwinkels
	 */
	public void drehtSichRichtig(double offset) {
		Fahrgestell fahrgestell = EHKBotMain.getInstance().getEHKBot().getFahrgestell();
		fahrgestell.dreheBot(new Richtung(0));
		double davor = IndoorGPS.getInstance().getRichtung().getAlsRad();
		double soll = davor + offset;
		soll = soll - (2 * Math.PI * Math.floor(soll / (2 * Math.PI)));
		fahrgestell.dreheBot(new Richtung(offset));
		double ist = IndoorGPS.getInstance().getRichtung().getAlsRad();
		double epsilon = 0.00001;
		String msg = String.format(
			"richtung hat sich nicht richtig um %f° geändert (soll%f ; is%f)",
			offset,
			soll,
			ist);
		assertTrue (msg , Math.abs(soll - ist) < epsilon);

	}

}
